/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Test;
import Behaviors.Arbitrators.*;
import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Utils.*;

import javaclient3.*;
import javaclient3.structures.*;

/**
 *
 * @author alejo
 */
public class pruebasNuevaAPI {
    
    public static void main(String args[])
    {

        boolean isSimulated = true;
        boolean showArbFrame = true;
        boolean showMapObstFrame = true;


        PlayerClient real;
        //Creacion de la instancia robot.
        if(isSimulated)
            real = new PlayerClient("localhost",6665);
        else
            real = new PlayerClient("192.168.2.146",6665);
        

        Position2DInterface posReal;
        SonarInterface sonar;
        
        posReal = real.requestInterfacePosition2D(0, PlayerConstants.PLAYER_OPEN_MODE);
        sonar = real.requestInterfaceSonar(0, PlayerConstants.PLAYER_OPEN_MODE);


        real.runThreaded(-1, -1);

        posReal.setMotorPower(1);

        PlayerPose initialPose;


        if(isSimulated)
            initialPose = new PlayerPose(-5.0f,-6.0f,Math.PI/4);
        else
            initialPose = new PlayerPose(0.0f,0.0f,0);


        posReal.setOdometry(initialPose);

        Position2DHelper posHelp = new Position2DHelper();

        if(isSimulated)
        {
            posHelp.setMaxSpeed(0.4f);
            posHelp.setRotControl(0.3f,1.0f);
        }
        else
        {
            posHelp.setMaxSpeed(1.0f);
            posHelp.setRotControl(0.2f,0.5f);
        }

        //comportamientos

        Vector2d point;

        if(isSimulated)
            point=new Vector2d(5,5,Vector2d.MODE_XY);
        else
            point=new Vector2d(2.0f,0.0f,Vector2d.MODE_XY);

        NxMSGoToPointNew goto2 = new NxMSGoToPointNew("goto",posReal,point,4, 0.2f, NxMSGoToPoint.MODE_EXPONENTIAL);
        goto2.setGain(1.0f);

        NxPSDetectObstaclesSonarNew obstReal = new NxPSDetectObstaclesSonarNew("obstReal",posReal, sonar, false  );
        obstReal.setMaxRange(2);
        obstReal.setMinRange(0.2f);

        NxPSMapObstaclesNew obstMapped = new NxPSMapObstaclesNew("mapObstacles",obstReal, posReal,2.0f,-1,showMapObstFrame);


        NxMSAvoidObstaclesNew avoidReal = new NxMSAvoidObstaclesNew("avoidReal",obstMapped, posReal,NxMSAvoidObstacles.MODE_EXPONENTIAL);
        avoidReal.setGain(0.4f);
        avoidReal.setExponentialDistance(0.3f);

        NxMSNavigationVFHNew navigate = new NxMSNavigationVFHNew("avoidVFH",obstMapped,goto2,posReal,NxMSAvoidObstacles.MODE_LINEAR,true);
        navigate.setGain(1.0f);
        navigate.setValleyTresh(1.3f, 0.7f);
        navigate.setExponentialDistance(0.3f);


        Behavior[] array = {navigate,avoidReal};

        CooperativeArbitratorNew arbitrator1 = new CooperativeArbitratorNew("a1",array,showArbFrame);
        
        arbitrator1.enabled = false;

        while(!posReal.isDataReady());
        while(!sonar.isDataReady());

        while(true)
        {
         
            Vector2d total = arbitrator1.Value();

            PlayerPose total1 = total.toPlayerPose();

            posHelp.goTo(total1, posReal);
            
            Position2DHelper.getPosition(posReal).print(Vector2d.MODE_XY);//posReal.getPosition().print(Vector2d.MODE_XY);
            try
            {
                Thread.sleep(50);
            }
            catch(Exception e)
            {}
        }
        
    }

}
